function plot_trajectory(X_history, X_ref, current_step, fig)
%PLOT_TRAJECTORY 实时绘制轨迹
%
% 输入:
%   X_history - 历史状态 (4 x N)
%   X_ref - 参考轨迹 (4 x N)
%   current_step - 当前步数
%   fig - 图形句柄

    figure(fig);
    clf;
    
    % XY轨迹
    subplot(2, 2, [1, 3]);
    plot(X_ref(1,:), X_ref(2,:), 'b--', 'LineWidth', 2, 'DisplayName', '参考轨迹');
    hold on;
    plot(X_history(1,1:current_step), X_history(2,1:current_step), ...
         'r-', 'LineWidth', 1.5, 'DisplayName', '实际轨迹');
    
    % 绘制车辆
    x_car = X_history(1, current_step);
    y_car = X_history(2, current_step);
    theta_car = X_history(3, current_step);
    
    % 简化的车辆形状
    car_length = 1.0;
    car_width = 0.5;
    car_x = [car_length/2, -car_length/2, -car_length/2, car_length/2, car_length/2];
    car_y = [car_width/2, car_width/2, -car_width/2, -car_width/2, car_width/2];
    
    % 旋转和平移
    R = [cos(theta_car), -sin(theta_car); sin(theta_car), cos(theta_car)];
    car_rotated = R * [car_x; car_y];
    car_x_global = car_rotated(1,:) + x_car;
    car_y_global = car_rotated(2,:) + y_car;
    
    plot(car_x_global, car_y_global, 'k-', 'LineWidth', 2);
    fill(car_x_global, car_y_global, 'g', 'FaceAlpha', 0.3);
    
    grid on;
    axis equal;
    xlabel('X (m)');
    ylabel('Y (m)');
    title(sprintf('轨迹跟踪 (步数: %d)', current_step));
    legend('Location', 'best');
    
    % 速度曲线
    subplot(2, 2, 2);
    plot(1:length(X_ref(4,:)), X_ref(4,:), 'b--', 'LineWidth', 2);
    hold on;
    plot(1:current_step, X_history(4,1:current_step), 'r-', 'LineWidth', 1.5);
    grid on;
    xlabel('时间步');
    ylabel('速度 (m/s)');
    title('速度跟踪');
    legend('参考', '实际');
    xlim([1, length(X_ref(4,:))]);
    
    % 航向角
    subplot(2, 2, 4);
    plot(1:length(X_ref(3,:)), X_ref(3,:), 'b--', 'LineWidth', 2);
    hold on;
    plot(1:current_step, X_history(3,1:current_step), 'r-', 'LineWidth', 1.5);
    grid on;
    xlabel('时间步');
    ylabel('航向角 (rad)');
    title('航向角跟踪');
    legend('参考', '实际');
    xlim([1, length(X_ref(3,:))]);
end